/*
   依赖库:
    MPU6050_tockn
    ArduinoJson
    WebSocketsServer
*/

#include <MPU6050_tockn.h>
#include <Wire.h>
#include <WiFi.h>
#include <WebSocketsServer.h>

const int led = 2;

// wifi 名称和路由
const char *ssid = "小亦站";
const char *password = "88889999";

// 设置静态 IP 配置
IPAddress local_ip(192, 168, 2, 192);  // 设置固定 IP 地址
IPAddress gateway(192, 168, 2, 1);     // 路由器网关地址
IPAddress subnet(255, 255, 255, 0);    // 子网掩码
int esp32Port = 82;

// 马达引脚
#define IN1 13
#define IN2 12
#define IN3 27
#define IN4 26

// 5路巡线传感器引脚定义（从左到右）
#define LINE_SENSOR_LEFT2 4
#define LINE_SENSOR_LEFT1 16
#define LINE_SENSOR_MIDDLE 17
#define LINE_SENSOR_RIGHT1 5
#define LINE_SENSOR_RIGHT2 18
int lineSensorValues[5];  // 存储5个巡线传感器的值

// 自定义eps32的I2C引脚
int sclPin = 21;
int sdaPin = 19;

// 小车基础属性
// 马达pwm补偿, 马达达到一定电压才开始转动, 参数值: 0 ~ 80
int lPwmOffset = 26;
int rPwmOffset = 26;
float angleOffset = -7;  // 角度补偿, 平衡站立时 mpu6050 实测角度, 参数值: -10 ~ 10

// PID
float kp = 9, ki = 0.12, kd = 0.16;  // pid 参数预设

// 前进后退
float targetAngle = 0;  // 目标角度, 直立状态下为 0, 更改可以前进或后退, 参数值: -3 ~ 3

// 转向
float turnKp = 0.1;     // 转向 pid 参数中的 turnKp , 简化省略了 turnKi 和 turnKd 参数, turnKp 参数值: 0.2 ~ 1
float turnSpeed = 0;  // 转向角速度, 参数值: 100 ~ 600

// 巡线, 参数值: 0 ~ 600
int useLineFollower = 0;                 // 是否开启巡线, 0 为不开启, 1 为开启
float lineTurnSpeed1 = 380;              // 巡线转速1, 参数值: 0 ~ 600
float lineTurnSpeed2 = 600;              // 巡线转速2, 参数值: 0 ~ 600
float lineAngle = 3;                     // 巡线前进角度
int blindTime = 1000;                     // 全白或全黑的时候继续盲走一定的时间, 单位毫秒
unsigned long currentMillis = millis();  // 获取当前毫秒数

// 全局变量, 全局创建一次, 在函数体内进行更改值, 避免在函数体内频繁下创建与销毁
float bias, integrate;                   // 角度偏差，偏差积分
float angleY, gyroY, gyroZ;              // 陀螺仪 mpu6050 的输出值, angleY: Y轴角度; gyroY: Y轴角速度; gyroZ: Z轴角速度
int verticalPwm, turnPwm, L_Pwm, R_Pwm;  //各种PWM计算值

MPU6050 mpu6050(Wire);  //实例化mpu6050对象

WebSocketsServer webSocket = WebSocketsServer(esp32Port);

// 读取所有巡线传感器的值, 返回 {直立pwm, 转向pwm}
void calculateLinePwm() {

  lineSensorValues[0] = digitalRead(LINE_SENSOR_LEFT2);
  lineSensorValues[1] = digitalRead(LINE_SENSOR_LEFT1);
  lineSensorValues[2] = digitalRead(LINE_SENSOR_MIDDLE);
  lineSensorValues[3] = digitalRead(LINE_SENSOR_RIGHT1);
  lineSensorValues[4] = digitalRead(LINE_SENSOR_RIGHT2);

  int counter = 0;
  for (int i = 0; i < 5; i++) {
    // 黑线为 0，白底为 1
    counter += lineSensorValues[i];
  }

  if (counter == 0 || counter == 5) {
    // 盲走
    if (millis() - currentMillis < blindTime) {
      turnSpeed = 0;
      targetAngle = lineAngle;
    } else {
      turnSpeed = 0;
      targetAngle = 0;
    }
    return;
  } else {
    currentMillis = millis();
    if (lineSensorValues[0] == 0) {
      turnSpeed = 0 - lineTurnSpeed2;
      targetAngle = 0;
    } else if (lineSensorValues[4] == 0) {
      turnSpeed = lineTurnSpeed2;
      targetAngle = 0;
    } else if (lineSensorValues[1] == 0) {
      turnSpeed = 0 - lineTurnSpeed1;
      targetAngle = 0;
    } else if (lineSensorValues[3] == 0) {
      turnSpeed = lineTurnSpeed1;
      targetAngle = 0;
    } else if (lineSensorValues[2] == 0) {
      turnSpeed = 0;
      targetAngle = lineAngle;
    }
  }
}

void motor(int left_EN, int right_EN)  //马达输出函数
{
  left_EN = constrain(left_EN, -255, 255);
  right_EN = constrain(right_EN, -255, 255);  //限定PWM区间在-255~255
  if (left_EN >= 0) {
    analogWrite(IN1, left_EN);
    analogWrite(IN2, 0);
  }
  if (left_EN < 0) {
    analogWrite(IN1, 0);
    analogWrite(IN2, 0 - left_EN);
  }
  if (right_EN >= 0) {
    analogWrite(IN3, 0);
    analogWrite(IN4, right_EN);
  }
  if (right_EN < 0) {
    analogWrite(IN3, 0 - right_EN);
    analogWrite(IN4, 0);
  }
}


void vertical_pwm_calculation()  //直立PMW计算
{
  angleY = mpu6050.getAngleY();
  gyroY = mpu6050.getGyroY();
  bias = angleY + angleOffset + targetAngle;      // 计算角度偏差。bias为小车角度是静态平衡角度的差值。
  integrate += bias;                              //偏差的积分，integrate为全局变量，一直积累。
  integrate = constrain(integrate, -1000, 1000);  //限定误差积分的最大和最小值
  /*==直立PID计算PWM==通过陀螺仪返回数据计算，前倾陀螺仪Y轴为正，后仰陀螺仪Y轴为负。
    前倾车前进，后仰车后退，保持直立。但可能为了直立，车会随时移动。*/
  verticalPwm = kp * bias + ki * integrate + kd * gyroY;
}

void turn_pwm_calculation()  //转向PMW计算
{
  gyroZ = mpu6050.getGyroZ();  //获取陀螺仪Z轴角速度
  turnPwm = turnKp * (turnSpeed + gyroZ);
  turnPwm = constrain(turnPwm, -180, 180);
}

void combine_pwm()  //马达PWM控制函数
{

  // 马达补偿
  if (verticalPwm >= 0) {
    L_Pwm = verticalPwm + lPwmOffset;
    R_Pwm = verticalPwm + rPwmOffset;
  }
  if (verticalPwm < 0) {
    L_Pwm = verticalPwm - lPwmOffset;
    R_Pwm = verticalPwm - rPwmOffset;
  }

  // 转向控制
  L_Pwm -= turnPwm;
  R_Pwm += turnPwm;

  // 限制输出
  L_Pwm = constrain(L_Pwm, -255, 255);
  R_Pwm = constrain(R_Pwm, -255, 255);
}

void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) {

  if (type == WStype_TEXT) {
    String msg = String((char *)payload);

    if (msg.startsWith("lPwmOffset:")) {  // 小车基础属性
      lPwmOffset = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("rPwmOffset:")) {
      rPwmOffset = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("angleOffset:")) {
      angleOffset = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("kp:")) {  // PID
      kp = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("ki:")) {
      ki = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("kd:")) {
      kd = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("targetAngle:")) {  // 前进后退
      targetAngle = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("turnKp:")) {  // 转向
      turnKp = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("turnSpeed:")) {
      turnSpeed = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("useLineFollower:")) {
      useLineFollower = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("lineTurnSpeed1:")) {  // 巡线
      lineTurnSpeed1 = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("lineTurnSpeed2:")) {
      lineTurnSpeed2 = msg.substring(msg.indexOf(":") + 1).toFloat();
    } else if (msg.startsWith("lineAngle:")) {
      lineAngle = msg.substring(msg.indexOf(":") + 1).toFloat();
    }
  }
}


void setup() {  //初始化

  Serial.begin(9600);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // 初始化巡线传感器引脚为输入模式
  pinMode(LINE_SENSOR_LEFT2, INPUT_PULLUP);
  pinMode(LINE_SENSOR_LEFT1, INPUT_PULLUP);
  pinMode(LINE_SENSOR_MIDDLE, INPUT_PULLUP);
  pinMode(LINE_SENSOR_RIGHT1, INPUT_PULLUP);
  pinMode(LINE_SENSOR_RIGHT2, INPUT_PULLUP);

  // 设置静态 IP 地址
  if (!WiFi.config(local_ip, gateway, subnet)) {
    Serial.println("配置静态 IP 失败!");
  }

  // 连接到 Wi-Fi 网络
  Serial.print("连接到 WiFi网络: ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);

  // 等待连接
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }

  // 输出连接的 IP 地址
  Serial.println();
  Serial.print("已连接到 WiFi, IP 地址：");
  Serial.println(WiFi.localIP());

  Wire.begin(sdaPin, sclPin);
  mpu6050.begin();
  mpu6050.calcGyroOffsets(false);
  motor(0, 0);

  webSocket.begin();
  webSocket.onEvent(webSocketEvent);

  delay(10);  //循环前延时，确保各种初始和准备完成
  currentMillis = millis();
}

void loop() {
  // WIFI调试+控制
  webSocket.loop();

  // 陀螺仪刷新
  mpu6050.update();

  // 直立环 PWM 计算, 更新全局变量: verticalPwm
  vertical_pwm_calculation();

  // 转向环 PWM 计算, 更新全局变量: turnPwm
  turn_pwm_calculation();

  // 巡线
  if (useLineFollower) { calculateLinePwm(); }

  // 结合 verticalPwm, turnPwm, leftMotorPwmOffset, rightMotorPwmOffset 更新全局变量: L_Pwm, R_Pwm
  combine_pwm();

  // 马达输出
  if (angleY > 45 || angleY < -45)  // 倾角过大（车倒下时），停止马达输出
  {
    motor(0, 0);
  } else {
    motor(L_Pwm, R_Pwm);  // 正常运转
  }

  // Serial.print(L_Pwm);
  // Serial.println(R_Pwm);
}
